﻿using System.Collections;
using System;
using System.Collections.Generic;
using UnityEngine;
using System.Threading;

namespace RVO3D
{
    /// <summary>
    /// 定义一个平面类
    /// </summary>
    public struct Plane
    {
        /// <summary>
        /// 平面上一点
        /// </summary>
        public Vector3 point;
        /// <summary>
        /// 平面的法线向量
        /// </summary>
        public Vector3 normal;
    }

    /// <summary>
    /// 包含所有模拟功能的库的主类
    /// </summary>
    public class RVOSimulator: MonoBehaviour
    {
        private class Worker
        {
            private ManualResetEvent doneEvent_;
            private int end_;
            private int start_;


            internal Worker(int start, int end, ManualResetEvent doneEvent)
            {
                start_ = start;
                end_ = end;
                doneEvent_ = doneEvent;
            }


            internal void step(object obj)
            {
                for (int agentNo = start_; agentNo < end_; ++agentNo)
                {
                    RVOSimulator.Instance.agents_[agentNo].ComputeNeighbors();
                    RVOSimulator.Instance.agents_[agentNo].ComputeNewVelocity();
                }

                doneEvent_.Set();
            }

            /**
             * <summary>updates the two-dimensional position and
             * two-dimensional velocity of each agent.</summary>
             *
             * <param name="obj">Unused.</param>
             */
            internal void update(object obj)
            {
                for (int agentNo = start_; agentNo < end_; ++agentNo)
                {
                    RVOSimulator.Instance.agents_[agentNo].Update();
                }

                doneEvent_.Set();
            }
        }

        public static RVOSimulator Instance { get; private set; }

        void Awake()
        {
            Instance = this;
        }

        private void Update()
        {
            SetTimeStep(Time.deltaTime);
            float a = DoStep();

            //Debug.Log(a);
        }


        private RVOSimulator()
        {
            Clear();
        }


        internal Agent defaultAgent_;
        internal KdTree kdTree_;
        internal float globalTime_;
        internal float timeStep_;

        private int numWorkers_;


        internal List<Agent> agents_;

        private ManualResetEvent[] doneEvents_;
        private Worker[] workers_;



        public void Clear()
        {
            agents_ = new List<Agent>();
            //obstacles_ = new List<Obstacle>();

            defaultAgent_ = null;
            kdTree_ = new KdTree();
            
            globalTime_ = 0.0f;
            timeStep_ = 0.1f;

            SetNumWorkers(0);
        }

        public void SetNumWorkers(int numWorkers)
        {
            numWorkers_ = numWorkers;

            if (numWorkers_ <= 0)
            {
                int completionPorts;
                ThreadPool.GetMinThreads(out numWorkers_, out completionPorts);
            }
            workers_ = null;
        }


        ///// <summary>
        ///// 构造一个模拟器实例，并为新Agent设置默认属性。
        ///// </summary>
        ///// <param name="timeStep">模拟的时间步长,必须是非负数</param>
        ///// <param name="neighborDist">默认的最大距离(中心点到中心点)到其他Agent的新Agent在导航中考虑。这个数字越大，模拟的运行时间越长。如果数值过低，模拟就不安全。必须是非负数。</param>
        ///// <param name="maxNeighbors">新Agent在导航中考虑的其他Agent的默认最大数量。这个数字越大，模拟的运行时间就越长。如果数值过低，模拟就不安全。</param>
        ///// <param name="timeHorizon">通过模拟计算的一个新的Agent速度相对于其他Agent来说是安全的默认最小时间量。这个数字越大，agent对其他agent的响应就越快，但是agent选择速度的自由就越少。必须是非负数。</param>
        ///// <param name="radius">新Agent的默认半径。必须是非负数。</param>
        ///// <param name="maxSpeed">新Agent的默认最大速度。必须是非负数。</param>
        ///// <param name="velocity">新Agent的默认初始三维线速度。</param>
        //public RVOSimulator(float timeStep, float neighborDist, int maxNeighbors, float timeHorizon, float radius, float maxSpeed,  Vector3 velocity)
        //{
        //    kdTree_ = new KdTree();
        //    defaultAgent_ = new Agent();
        //    defaultAgent_.maxNeighbors_ = maxNeighbors;
        //    defaultAgent_.maxSpeed_ = maxSpeed;
        //    defaultAgent_.neighborDist_ = neighborDist;
        //    defaultAgent_.radius_ = radius;
        //    defaultAgent_.timeHorizon_ = timeHorizon;
        //    defaultAgent_.velocity_ = velocity;    
        //}

        /// <summary>
        /// 在模拟中添加一个具有默认属性的新Agent，返回Agent编号
        /// </summary>
        /// <param name="position">此Agent的起始位置</param>
        /// <returns></returns>
        public int AddAgent(Vector3 position)
        {
            if (defaultAgent_ == null)
            {
                return Math.RVO3D_ERROR;
            }
            Agent agent = new Agent();
            agent.position_ = position;
            agent.maxNeighbors_ = defaultAgent_.maxNeighbors_;
            agent.maxSpeed_ = defaultAgent_.maxSpeed_;
            agent.neighborDist_ = defaultAgent_.neighborDist_;
            agent.radius_ = defaultAgent_.radius_;
            agent.timeHorizon_ = defaultAgent_.timeHorizon_;
            agent.velocity_ = defaultAgent_.velocity_;
            agent.id_ = agents_.Count;
            agents_.Add(agent);
            return agents_.Count - 1;
        }

        /// <summary>
        /// 向模拟中添加新Agent，并返回他的编号
        /// </summary>
        /// <param name="position">Agent的起始位置</param>
        /// <param name="neighborDist">默认的最大距离(中心点到中心点)到其他Agent的新Agent在导航中考虑。这个数字越大，模拟的运行时间越长。如果数值过低，模拟就不安全。必须是非负数。</param>
        /// <param name="maxNeighbors">新Agent在导航中考虑的其他Agent的默认最大数量。这个数字越大，模拟的运行时间就越长。如果数值过低，模拟就不安全。</param>
        /// <param name="timeHorizon">通过模拟计算的一个新的Agent速度相对于其他Agent来说是安全的默认最小时间量。这个数字越大，agent对其他agent的响应就越快，但是agent选择速度的自由就越少。必须是非负数。</param>
        /// <param name="radius">新Agent的默认半径。必须是非负数。</param>
        /// <param name="maxSpeed">新Agent的默认最大速度。必须是非负数。</param>
        /// <param name="velocity">新Agent的默认初始三维线速度。</param>
        /// <returns></returns>
        public int AddAgent(Vector3 position, float neighborDist, int maxNeighbors, float timeHorizon, float radius, float maxSpeed, Vector3 velocity)
        {
            Agent agent = new Agent();
            agent.position_ = position;
            agent.maxNeighbors_ = maxNeighbors;
            agent.maxSpeed_ = maxSpeed;
            agent.neighborDist_ = neighborDist;
            agent.radius_ = radius;
            agent.timeHorizon_ = timeHorizon;
            agent.velocity_ = velocity;
            agent.id_ = agents_.Count;
            agents_.Add(agent);
            return agents_.Count - 1;
        }


        /// <summary>
        /// 模拟器执行一个仿真步骤，更新每个Agent的三维位置和三维速度。
        /// </summary>
        public float DoStep()
        {
            if (workers_ == null)
            {
                workers_ = new Worker[numWorkers_];
                doneEvents_ = new ManualResetEvent[workers_.Length];

                for (int block = 0; block < workers_.Length; ++block)
                {
                    doneEvents_[block] = new ManualResetEvent(false);
                    workers_[block] = new Worker(block * GetNumAgents() / workers_.Length, (block + 1) * GetNumAgents() / workers_.Length, doneEvents_[block]);
                }
            }

            kdTree_.BuildAgentTree();

            for (int block = 0; block < workers_.Length; ++block)
            {
                doneEvents_[block].Reset();
                ThreadPool.QueueUserWorkItem(workers_[block].step);
            }

            WaitHandle.WaitAll(doneEvents_);

            for (int block = 0; block < workers_.Length; ++block)
            {
                doneEvents_[block].Reset();
                ThreadPool.QueueUserWorkItem(workers_[block].update);
            }

            WaitHandle.WaitAll(doneEvents_);

            globalTime_ += timeStep_;

            return globalTime_;
        }


        /// <summary>
        /// 返回指定Agent的相邻Agent的编号。
        /// </summary>
        /// <param name="agentNo">指定的Agent的编号</param>
        /// <param name="neighborNo">相邻Agent的编号</param>
        /// <returns></returns>
        public int GetAgentAgentNeighbor(int agentNo, int neighborNo)
        {
            return agents_[agentNo].agentNeighbors_[neighborNo].Value.id_;
        }

        /// <summary>
        /// 返回指定Agent的最大邻居数量
        /// </summary>
        /// <param name="agentNo">要获取最大邻居数量的Agent编号</param>
        /// <returns></returns>
        public int GetAgentMaxNeighbors(int agentNo)
        {
            return agents_[agentNo].maxNeighbors_;
        }

        /// <summary>
        /// 返回指定Agent的最大速度
        /// </summary>
        /// <param name="agentNo"></param>
        /// <returns></returns>
        public float GetAgentMaxSpeed(int agentNo)
        {
            return agents_[agentNo].maxSpeed_;
        }

        /// <summary>
        /// 返回指定Agent的最大邻居距离
        /// </summary>
        /// <param name="agentNo"></param>
        /// <returns></returns>
        public float GetAgentNeighborDist(int agentNo)
        {
            return agents_[agentNo].neighborDist_;
        }

        /// <summary>
        /// 返回考虑到Agent邻居的数量，计算指定Agent的当前速度。
        /// </summary>
        /// <param name="agentNo"></param>
        /// <returns></returns>
        public int GetAgentNumAgentNeighbors(int agentNo)
        {
            return agents_[agentNo].agentNeighbors_.Count;
        }

        /// <summary>
        /// 返回用于计算指定Agent当前速度的ORCA约束的数量。
        /// </summary>
        /// <param name="agentNo"></param>
        /// <returns></returns>
        public int GetAgentNumORCAPlanes(int agentNo)
        {
            return agents_[agentNo].orcaPlanes_.Count;
        }



        public Plane GetAgentORCAPlane(int agentNo, int planeNo)
        {
            return agents_[agentNo].orcaPlanes_[planeNo];
        }

        public Vector3 GetAgentPosition(int agentNo)
        {
            return agents_[agentNo].position_;
        }

        public Vector3 GetAgentPrefVelocity(int agentNo)
        {
            return agents_[agentNo].prefVelocity_;
        }

        public float GetAgentRadius(int agentNo)
        {
            return agents_[agentNo].radius_;
        }

        public float GetAgentTimeHorizon(int agentNo)
        {
            return agents_[agentNo].timeHorizon_;
        }

        public Vector3 GetAgentVelocity(int agentNo)
        {
            return agents_[agentNo].velocity_;
        }

        public float GetGlobalTime()
        {
            return globalTime_;
        }

        public int GetNumAgents()
        {
            return agents_.Count;
        }

        public float GetTimeStep()
        {
            return timeStep_;
        }

        public void removeAgent(int agentNo)
        {
            agents_[agentNo] = agents_[agents_.Count - 1];
            agents_.RemoveAt(agents_.Count - 1);
        }



        public void SetAgentDefaults(float neighborDist, int maxNeighbors, float timeHorizon, float radius, float maxSpeed, Vector3 velocity)
        {
            if (defaultAgent_ == null)
            {
                defaultAgent_ = new Agent();
            }
            defaultAgent_.maxNeighbors_ = maxNeighbors;
            defaultAgent_.maxSpeed_ = maxSpeed;
            defaultAgent_.neighborDist_ = neighborDist;
            defaultAgent_.radius_ = radius;
            defaultAgent_.timeHorizon_ = timeHorizon;
            defaultAgent_.velocity_ = velocity;
        }

        public void SetAgentMaxNeighbors(int agentNo, int maxNeighbors)
        {
            agents_[agentNo].maxNeighbors_ = maxNeighbors;
        }

        public void SetAgentMaxSpeed(int agentNo, int maxSpeed)
        {
            agents_[agentNo].maxSpeed_ = maxSpeed;
        }

        public void SetAgentNeighborDist(int agentNo, float neighborDist)
        {
            agents_[agentNo].neighborDist_ = neighborDist;
        }

        public void SetAgentPosition(int agentNo, Vector3 position)
        {
            agents_[agentNo].position_ = position;
        }

        public void SetAgentPrefVelocity(int agentNo, Vector3 prefVelocity)
        {
            agents_[agentNo].prefVelocity_ = prefVelocity;
        }

        public void SetAgentRadius(int agentNo, float radius)
        {
            agents_[agentNo].radius_ = radius;
        }

        public void SetAgentTimeHorizon(int agentNo, float timeHorizon)
        {
            agents_[agentNo].timeHorizon_ = timeHorizon;
        }

        public void SetAgentVelocity(int agentNo, Vector3 velocity)
        {
            agents_[agentNo].velocity_ = velocity;
        }

        public void SetTimeStep(float timeStep)
        {
            timeStep_ = timeStep;
        }

    }
}
